/*****************************************************************************
*  Copyright Statement:
*  --------------------
*  This software is protected by Copyright and the information contained
*  herein is confidential. The software may not be copied and the information
*  contained herein may not be used or disclosed except with the written
*  permission of MediaTek Inc. (C) 2005
*
*  BY OPENING THIS FILE, BUYER HEREBY UNEQUIVOCALLY ACKNOWLEDGES AND AGREES
*  THAT THE SOFTWARE/FIRMWARE AND ITS DOCUMENTATIONS ("MEDIATEK SOFTWARE")
*  RECEIVED FROM MEDIATEK AND/OR ITS REPRESENTATIVES ARE PROVIDED TO BUYER ON
*  AN "AS-IS" BASIS ONLY. MEDIATEK EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES,
*  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED WARRANTIES OF
*  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR NONINFRINGEMENT.
*  NEITHER DOES MEDIATEK PROVIDE ANY WARRANTY WHATSOEVER WITH RESPECT TO THE
*  SOFTWARE OF ANY THIRD PARTY WHICH MAY BE USED BY, INCORPORATED IN, OR
*  SUPPLIED WITH THE MEDIATEK SOFTWARE, AND BUYER AGREES TO LOOK ONLY TO SUCH
*  THIRD PARTY FOR ANY WARRANTY CLAIM RELATING THERETO. MEDIATEK SHALL ALSO
*  NOT BE RESPONSIBLE FOR ANY MEDIATEK SOFTWARE RELEASES MADE TO BUYER'S
*  SPECIFICATION OR TO CONFORM TO A PARTICULAR STANDARD OR OPEN FORUM.
*
*  BUYER'S SOLE AND EXCLUSIVE REMEDY AND MEDIATEK'S ENTIRE AND CUMULATIVE
*  LIABILITY WITH RESPECT TO THE MEDIATEK SOFTWARE RELEASED HEREUNDER WILL BE,
*  AT MEDIATEK'S OPTION, TO REVISE OR REPLACE THE MEDIATEK SOFTWARE AT ISSUE,
*  OR REFUND ANY SOFTWARE LICENSE FEES OR SERVICE CHARGE PAID BY BUYER TO
*  MEDIATEK FOR SUCH MEDIATEK SOFTWARE AT ISSUE.
*
*  THE TRANSACTION CONTEMPLATED HEREUNDER SHALL BE CONSTRUED IN ACCORDANCE
*  WITH THE LAWS OF THE STATE OF CALIFORNIA, USA, EXCLUDING ITS CONFLICT OF
*  LAWS PRINCIPLES.  ANY DISPUTES, CONTROVERSIES OR CLAIMS ARISING THEREOF AND
*  RELATED THERETO SHALL BE SETTLED BY ARBITRATION IN SAN FRANCISCO, CA, UNDER
*  THE RULES OF THE INTERNATIONAL CHAMBER OF COMMERCE (ICC).
*
*****************************************************************************/

/*****************************************************************************
 *
 * Filename:
 * ---------
 *   mpu_9250_i2c.c
 *
 *
 * Description:
 * ------------
 *   Capacitive Touch Panel I2C read/write Driver
 *
 * Author:
 * -------

 *
 *------------------------------------------------------------------------------
 * Upper this line, this part is controlled by PVCS VM. DO NOT MODIFY!!
 *============================================================================
 *****************************************************************************/

#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>
#include "mpu_9250_i2c.h"
#include "hd_i2c.h"

static int g_fd = -1;



extern void mpu9250_i2c_udelay(unsigned int delay)
{
	usleep(delay);
}

int mpu9250_i2c_configure(unsigned int slave_addr, unsigned int speed)
{
    int ret;

    g_fd = open(MPU9250_DEV, O_RDWR);
    if(g_fd < 0)
    {
        printf("Open %s error!\n", MPU9250_DEV);
        return -1;
    }


    printf("open %s sucucess g_fd %d\n", MPU9250_DEV, g_fd);
	
    return 0;
}

int mpu9250_i2c_send(unsigned char dev_addr, unsigned char reg_addr, unsigned char* p_data, unsigned int data_len)
{
    return hd_i2c_nbytes_write(g_fd, dev_addr, reg_addr, p_data, data_len);
}


// I2C receive data function
int mpu9250_i2c_receive(unsigned char dev_addr, unsigned char reg_addr, unsigned char* p_data, unsigned int reg_off)
{
    return hd_i2c_nbytes_read(g_fd, dev_addr, reg_addr, p_data, reg_off);	
}

//MPU9250硬件初始化
void mpu9250_HW_init()
{
    unsigned char send_data = 0;
    unsigned char id = 0;
    mpu9250_i2c_configure(MPU9250_ADDR,200);

    send_data = 0x80;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_PWR_MGMT1_REG,&send_data,1); //复位MP9250
    mpu9250_i2c_udelay(1000);

    send_data = 0x00;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_PWR_MGMT1_REG,&send_data,1); //唤醒MP9250
    mpu9250_i2c_udelay(1000);

    send_data = 0x03<<3;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_GYRO_CFG_REG,&send_data,1);  //陀螺仪传感器,+-2000dps
    mpu9250_i2c_udelay(1000);

    send_data = 0x00<<3;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_ACCEL_CFG_REG,&send_data,1); //加速度传感器，+-2g
    mpu9250_i2c_udelay(1000);

    MPU_Set_Rate(50);                                              //设置采样率50Hz

    send_data = 0x00;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_INT_EN_REG,&send_data,1);    //关闭所有中断
    mpu9250_i2c_udelay(1000);
    mpu9250_i2c_send(MPU9250_ADDR,MPU_USER_CTRL_REG,&send_data,1); //关闭I2C主模式
    mpu9250_i2c_udelay(1000);
    mpu9250_i2c_send(MPU9250_ADDR,MPU_FIFO_EN_REG,&send_data,1);   //关闭FIFO
    mpu9250_i2c_udelay(1000);

    send_data = 0x82;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_INTBP_CFG_REG,&send_data,1); //INT引脚低电平有效,开启bypass模式，可直接读取磁力计
    mpu9250_i2c_udelay(1000);

    mpu9250_i2c_receive(MPU9250_ADDR,MPU_DEVICE_ID_REG,&id,1);     //读取MPU6500的ID
    if(id == MPU6500_ID)
        printf("mpu9250_i2c_receive MPU6500_ID=%d",id);
    mpu9250_i2c_udelay(1000);

    send_data = 0x01;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_PWR_MGMT1_REG,&send_data,1); //设置CLKSEL,PLL x轴为参考
    mpu9250_i2c_udelay(1000);

    send_data = 0x00;
    mpu9250_i2c_send(MPU9250_ADDR,MPU_PWR_MGMT2_REG,&send_data,1); //加速度与陀螺仪都工作
    mpu9250_i2c_udelay(1000);
#if 1 
    mpu9250_i2c_receive(AK8963_ADDR,MAG_WIA,&id,1);                //读取AK8963的ID
    if(id == AK8963_ID)
        printf("mpu9250_i2c_receive AK8963_ID=%d",id);
    mpu9250_i2c_udelay(1000);

    send_data = 0x11;
    mpu9250_i2c_send(AK8963_ADDR,MAG_CNTL1,&send_data,1); //设置AK8963为单次测量模式
    mpu9250_i2c_udelay(1000);
#endif

    printf("mpu9250_HW_init sucucess\n");
}

void mpu9250_HW_exit()
{
    if(g_fd != 0)
    {
        close(g_fd);
    }
}

//设置MPU9250的数字低通滤波器
unsigned char MPU_Set_LPF(unsigned short lpf)
{
	unsigned char data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6;
	return mpu9250_i2c_send(MPU9250_ADDR, MPU_CFG_REG, &data, 1);
}


//设置MPU9250的采样率
unsigned char MPU_Set_Rate(unsigned short rate)
{
	unsigned char data;
	if(rate > 1000)
        rate = 1000;
	if(rate < 4)
        rate = 4;
	data = 1000 / rate -1;
	data = mpu9250_i2c_send(MPU9250_ADDR, MPU_SAMPLE_RATE_REG, &data, 1);
 	return MPU_Set_LPF(rate/2);
}


//获取温度值(返回值扩大了100倍)
short MPU_Get_Temperature(void)
{
    unsigned char buf[2];
    short raw;
	float temp;
	mpu9250_i2c_receive(MPU9250_ADDR,MPU_TEMP_OUTH_REG,buf,2);
    //printf("buf%x %x\n", buf[0], buf[1]);	
    raw=((unsigned short)buf[0]<<8)|buf[1];
    temp=21+((double)raw)/333.87;
    return temp*100;
}

//获取陀螺仪值
unsigned char MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    unsigned char buf[6],res;
	res=mpu9250_i2c_receive(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,buf,6);
	if(res>0)
	{
		*gx=((unsigned short)buf[0]<<8)|buf[1];
		*gy=((unsigned short)buf[2]<<8)|buf[3];
		*gz=((unsigned short)buf[4]<<8)|buf[5];
	}
   // printf("buf%x %x %x %x %x %x\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
    return res;;
}

//获取加速度值
unsigned char MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    unsigned char buf[6],res;
	res=mpu9250_i2c_receive(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,buf,6);
	if(res>0)
	{
		*ax=((unsigned short)buf[0]<<8)|buf[1];
		*ay=((unsigned short)buf[2]<<8)|buf[3];
		*az=((unsigned short)buf[4]<<8)|buf[5];
	}
    return res;
}

//获取磁力计值
unsigned char MPU_Get_Magnetometer(short *mx,short *my,short *mz)
{
    unsigned char buf[6],res;
    unsigned char data;
	res=mpu9250_i2c_receive(AK8963_ADDR,MAG_XOUT_L,buf,6);
	if(res>0)
	{
		*mx=((unsigned short)buf[1]<<8)|buf[0];
		*my=((unsigned short)buf[3]<<8)|buf[2];
		*mz=((unsigned short)buf[5]<<8)|buf[4];
	}
    data = 0x11;
    mpu9250_i2c_send(AK8963_ADDR,MAG_CNTL1,&data,1); //AK8963每次读完以后都重设单次测量模式
}



